#include "Hall.h"

#define WHEEL_PERIMETER    67.0
#define PULSE_PER_CIRCLE   69
#define UPDATE_TIME        100

static volatile uint32_t hall_left_pulse = 0;
static volatile uint32_t hall_right_pulse = 0;
static uint32_t hall_left_speed = 0;
static uint32_t hall_right_speed = 0;

static void Hall_GPIO_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE);
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
}

static void Hall_EXTI_Init(void)
{
    EXTI_InitTypeDef EXTI_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource5);
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource6);
    EXTI_InitStructure.EXTI_Line = EXTI_Line5 | EXTI_Line6;
    EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
    EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
    EXTI_InitStructure.EXTI_LineCmd = ENABLE;
    EXTI_Init(&EXTI_InitStructure);
    NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
}

static void Hall_Timer_Init(void)
{
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    NVIC_InitTypeDef NVIC_InitStructure;
    
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    
    TIM_TimeBaseStructure.TIM_Period = 7199;
    TIM_TimeBaseStructure.TIM_Prescaler = 999;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    
    TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
    
    NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    
    TIM_Cmd(TIM4, ENABLE);
}

void Hall_Init(void)
{
    Hall_GPIO_Init();
    Hall_EXTI_Init();
    Hall_Timer_Init();
}

void Hall_GetPulse(uint32_t *left, uint32_t *right)
{
    *left = hall_left_pulse;
    *right = hall_right_pulse;
}

void Hall_GetSpeed(uint32_t *left_speed, uint32_t *right_speed)
{
    *left_speed = hall_left_speed;
    *right_speed = hall_right_speed;
}

void Hall_ClearPulse(void)
{
    hall_left_pulse = 0;
    hall_right_pulse = 0;
}

void Hall_Update(void)
{
    float left_distance = hall_left_pulse * (WHEEL_PERIMETER / PULSE_PER_CIRCLE);
    float right_distance = hall_right_pulse * (WHEEL_PERIMETER / PULSE_PER_CIRCLE);
    
    hall_left_speed = (uint32_t)(left_distance * 1000 / UPDATE_TIME);
    hall_right_speed = (uint32_t)(right_distance * 1000 / UPDATE_TIME);
    
    Hall_ClearPulse();
}

void EXTI9_5_IRQHandler(void)
{
    if(EXTI_GetITStatus(EXTI_Line5) != RESET)
    {
        if(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5) == 1)
        {
            hall_left_pulse++;
        }
        EXTI_ClearITPendingBit(EXTI_Line5);
    }
    if(EXTI_GetITStatus(EXTI_Line6) != RESET)
    {
        if(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6) == 1)
        {
            hall_right_pulse++;
        }
        EXTI_ClearITPendingBit(EXTI_Line6);
    }
}

void TIM4_IRQHandler(void)
{
    if(TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
    {
        Hall_Update();
        TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
    }
}
